import getch
import redis
import time
import subprocess
import os
from multiprocessing import Process
import json
import pickle
import geopy.distance
from datetime import datetime

r = redis.Redis(db=0, socket_timeout=1)

switch = True


def keyboard_input():
    while True:
        char = getch.getch()
        print(char)

        if char == ' ':
            char = 'Key.space'
        elif char == '1':
            if r.get('process_1').decode("utf-8") == 'off':
                r.set('process_1', 'on')
                p = subprocess.Popen('nohup /home/asmaa/miniconda3/envs/slam_drive/bin/python ~/Documents/asmaa/radar/height_control.py >/dev/null 2>&1 &',shell=True)
            else:
                r.set('process_1', 'off')
        elif char == '2':
            if r.get('process_2').decode("utf-8") == 'off':
                r.set('process_2', 'on')
                p = subprocess.Popen('nohup /home/asmaa/miniconda3/envs/slam_drive/bin/python ~/Documents/asmaa/control/advanceControl_2.py >/dev/null 2>&1 &', shell=True)
            else:
                r.set('process_2', 'off')
        elif char == '3':
            if r.get('process_3').decode("utf-8") == 'off':
                r.set('process_3', 'on')
                p = subprocess.Popen('nohup /home/asmaa/miniconda3/envs/slam_drive/bin/python ~/Documents/asmaa/gps/read_gps.py >/dev/null 2>&1 &', shell=True)
            else:
                r.set('process_3', 'off')
        elif char == '4':
            if r.get('process_4').decode("utf-8") == 'off':
                r.set('process_4', 'on')
                p = subprocess.Popen(
                    'nohup /home/asmaa/miniconda3/envs/slam_drive/bin/python ~/Documents/asmaa/zed/position_example.py >/dev/null 2>&1 &', shell=True)
            else:
                r.set('process_4', 'off')
        elif char == '5':
            if r.get('process_5').decode("utf-8") == 'off':
                r.set('process_5', 'on')
                p = subprocess.Popen('nohup /home/asmaa/miniconda3/envs/slam_drive/bin/python ~/Documents/asmaa/control/test.py >/dev/null 2>&1 &',
                                     shell=True)
            else:
                r.set('process_5', 'off')
        elif char == '6':
            if r.get('process_6').decode("utf-8") == 'off':
                r.set('process_6', 'on')
                p = subprocess.Popen(
                    'nohup /home/asmaa/miniconda3/envs/slam_drive/bin/python ~/Documents/asmaa/control/slam_auto.py >/dev/null 2>&1 &', shell=True)
            else:
                r.set('process_6', 'off')
        elif char == '7':
            if r.get('process_7').decode("utf-8") == 'off':
                r.set('process_7', 'on')
                p = subprocess.Popen(
                    'nohup /home/asmaa/miniconda3/envs/slam_drive/bin/python ~/Documents/asmaa/radar/radar_1.py >/dev/null 2>&1 &', shell=True)
            else:
                r.set('process_7', 'off')

        r.set('user_move_timestamp', int(time.time()))
        r.set('keyboard_input', char)

        if char == '/':
            break


def keyboard_control():
    p = Process(target=keyboard_input)
    p.start()

    while True:
        os.system('clear')
        try:
            print('operation_control[1] \t%s\tstatus: %s' % (r.get('process_1').decode("utf-8"), r.get('process_1_status').decode("utf-8")))
            print('advanceControl   [2] \t%s\tleft: %s\t right: %s ' % (r.get('process_2').decode("utf-8"), r.get('forward_left_speed').decode("utf-8"), r.get('forward_right_speed').decode("utf-8")))
            print('read_gps         [3] \t%s\tvAcc: %s\t%s' % (r.get('process_3').decode("utf-8"), json.loads(r.lindex('gps_queue', 0))[3], json.loads(r.lindex('gps_queue', 0))[1]))
            print('position_example [4] \t%s\t%s' % (r.get('process_4').decode("utf-8"), json.loads(r.lindex('vio_queue', 0))))
            print('fusion           [5] \t%s\t%s' % (r.get('process_5').decode("utf-8"), r.get('process_5_log').decode("utf-8")))
            print('auto_control     [6] \t%s\t%s' % (r.get('process_6').decode("utf-8"), r.get('process_6_log').decode("utf-8")))
            print('radar            [7] \t%s\t%s' % (r.get('process_7').decode("utf-8"), r.get('process_7_log').decode("utf-8")))
            print(datetime.now().strftime("%H:%M:%S:%f"))

        except TypeError:
            pass

        key = r.get('keyboard_input').decode("utf-8")

        if key == '/':
            break

        print(key)
        time.sleep(0.5)



    p.join()


def job_queue_tool():
    global switch

    while True:
        keys = input('>>>>>>>')

        if keys == '/':
            break
        elif keys == '//':
            switch = False
            break
        if keys.startswith('combo'):
            combo_name = keys.split(' ')[1]
            exec("[text_processing(x) for x in %s]" % combo_name)

        else:
            text_processing(keys)

        print(r.lrange('job_queue', 0, 10))


def text_processing(keys):
    with open('/home/asmaa/Documents/asmaa/control/waypoints.json', 'r') as fp:
        gps_location = json.load(fp)

    if ',' in keys:
        points = keys.split(',')
        if len(points) == 3:
            r.rpush('job_queue', json.dumps(['line', [points[0], points[1], int(points[2])]]))
        else:
            r.rpush('job_queue', json.dumps(['line', [points[0], points[1], 0]]))

    elif keys == 'clr':
        r.delete('job_queue')
    elif keys == 'lt':
        r.rpush('job_queue', json.dumps(['fix', [1000, 6000, 9]]))
    elif keys == 'rt':
        r.rpush('job_queue', json.dumps(['fix', [6000, 1000, 9]]))
    elif keys == 'ls':
        r.rpush('job_queue', json.dumps(['fix', [-3000, 3000, 7.5]]))
    elif keys == 'rs':
        r.rpush('job_queue', json.dumps(['fix', [3000, -3000, 7]]))
    elif keys == 'ff':
        r.rpush('job_queue', json.dumps(['fix', [3000, 3000, 3.5]]))
    elif keys == 'down':
        r.rpush('job_queue', json.dumps(['roll_down', []]))
    elif keys == 'up':
        r.rpush('job_queue', json.dumps(['roll_up', []]))
    elif keys.startswith('set-th'):
        threshold = keys.split(' ')[1]
        r.set('threshold', int(threshold))
        # r.rpush('job_queue', json.dumps(['set_th', [int(threshold)]]))

    elif keys.startswith('record'):
        name = keys.split(' ')[1]
        r.rpush('job_queue', json.dumps(['record_gps', [name]]))
    elif keys.startswith('show'):
        name = keys.split(' ')[1]
        print(gps_location[name]['cor'])
    elif keys.startswith('manual_record'):
        name = keys.split(' ')[1]
        lat = float(keys.split(' ')[2])
        lon = float(keys.split(' ')[3])
        gps_location[name] = {'cor': [lat, lon]}

        with open('/home/asmaa/Documents/asmaa/control/waypoints.json', 'w') as fp:
            json.dump(gps_location, fp)

    elif keys.startswith('lines'):
        points = keys.split(' ')[1:]

        index = 0

        while index < len(points) - 1:
            if index != 0:
                start_point = gps_location[points[index - 1]]['cor']
                end_point = gps_location[points[index]]['cor']
                next_point = gps_location[points[index + 1]]['cor']

                d = (next_point[0] - start_point[0]) * (end_point[1] - start_point[1]) - (
                            next_point[1] - start_point[1]) * (
                            end_point[0] - start_point[0])

                a = geopy.distance.distance((start_point[0], start_point[1]), (next_point[0], next_point[1])).m
                b = geopy.distance.distance((end_point[0], end_point[1]), (next_point[0], next_point[1])).m
                c = geopy.distance.distance((start_point[0], start_point[1]), (end_point[0], end_point[1])).m

                cos = (b ** 2 + c ** 2 - a ** 2) / (2 * b * c)

                if abs(cos) < 0.5:
                    if d > 0:
                        # 'left'
                        text_processing('ff')
                        text_processing('ls')
                    else:
                        # 'right'
                        text_processing('ff')
                        text_processing('rs')

            text_processing('%s,%s' % (points[index], points[index + 1]))
            index = index + 1

    elif keys.startswith('path'):
        path = {
            'r1': ['r6', 'r4', 'r5', 'r9'],
            'r2': ['r9', 'r3'],
            'r3': ['r2'],
            'r6': ['r1', 'r10'],
            'r4': ['r1'],
            'r5': ['r1'],
            'r9': ['stop', 'r2', 'r1'],
            'stop': ['r9'],
            'r10': ['r6']
        }

        start = keys.split(' ')[1]
        end = keys.split(' ')[2]

        path_list = []

        def search(node, target):
            if node == target:
                path_list.append(node)
                return 1
            else:
                path_list.append(node)
                for n in path[node]:
                    if n not in path_list:
                        if search(n, target):
                            return 1
                path_list.pop()

        search(start, end)
        print(path_list)

        text_processing(' '.join(['lines'] + path_list))

    elif keys == 'left_adjust':
        r.rpush('job_queue', json.dumps(['left_adjust', []]))
    elif keys == 'right_adjust':
        r.rpush('job_queue', json.dumps(['right_adjust', []]))
    elif keys.startswith('search'):
        search_item = keys.split(' ')[1]
        print([x for x in list(gps_location_list.keys()) if search_item in x])
    elif keys.startswith('vslam_reverse'):
        startpoint = keys.split(' ')[1]
        endpoint = keys.split(' ')[2]

        if len(keys.split(' ')) == 4:
            no_stop = 1
        else:
            no_stop = 0

        r.rpush('job_queue',
                json.dumps(['reverse_line', [greenhouse_points[startpoint], greenhouse_points[endpoint], no_stop]]))

    elif keys.startswith('vslam_line'):
        startpoint = keys.split(' ')[1]
        endpoint = keys.split(' ')[2]
        if len(keys.split(' ')) == 4:
            no_stop = 1
        else:
            no_stop = 0

        r.rpush('job_queue',
                json.dumps(['openvslam_line', [greenhouse_points[startpoint], greenhouse_points[endpoint], no_stop]]))
    elif keys.startswith('fix'):
        left_speed = keys.split(' ')[1]
        right_speed = keys.split(' ')[2]
        distance_lock = keys.split(' ')[3]
        r.rpush('job_queue', json.dumps(['fix_speed', [int(left_speed), int(right_speed), float(distance_lock)]]))
    elif keys.startswith('map_open'):
        map_file = keys.split(' ')[1]
        r.rpush('job_queue', json.dumps(['map_open', [map_file]]))
    elif keys.startswith('map_close'):
        r.rpush('job_queue', json.dumps(['map_close', []]))
    elif keys.startswith('roll_down'):
        r.rpush('job_queue', json.dumps(['roll_down', []]))
    elif keys.startswith('roll_up'):
        r.rpush('job_queue', json.dumps(['roll_up', []]))
    elif keys.startswith('set_th'):
        threshold = keys.split(' ')[1]
        r.rpush('job_queue', json.dumps(['set_th', [int(threshold)]]))
    elif keys.startswith('chain'):
        lane_name = keys.split(' ')[1]
        points_list = [x for x in list(gps_location_list.keys()) if x.startswith(lane_name)]

        points_list.sort(key=natural_keys)

        if len(keys.split(' ')) == 3:
            offset = int(keys.split(' ')[2])
        else:
            offset = 0

        index = 0

        while index < len(points_list) - 1:
            r.rpush('job_queue', json.dumps(['line', [points_list[index], points_list[index + 1], offset]]))
            index = index + 1
    elif keys.startswith('reverse_chain'):
        lane_name = keys.split(' ')[1]
        points_list = [x for x in list(gps_location_list.keys()) if x.startswith(lane_name)]

        points_list.sort(key=natural_keys, reverse=True)

        if len(keys.split(' ')) == 3:
            offset = int(keys.split(' ')[2])
        else:
            offset = 0

        index = 0

        while index < len(points_list) - 1:
            r.rpush('job_queue', json.dumps(['line', [points_list[index], points_list[index + 1], offset]]))
            index = index + 1
    elif keys == 'pop':
        r.lpop('job_queue')
    elif keys.startswith('op'):
        code = keys.split(' ')[1]
        time_to_sleep = keys.split(' ')[2]
        r.rpush('job_queue', json.dumps(['operation', [code, time_to_sleep]]))
    elif keys.startswith('thread_op'):
        code = keys.split(' ')[1]
        time_to_sleep = keys.split(' ')[2]
        r.rpush('job_queue', json.dumps(['operation_thread', [code, time_to_sleep]]))


if __name__ == '__main__':
    # r.set('keyboard_input', 'Key.space')

    while switch:
        keyboard_control()
        job_queue_tool()
        r.set('keyboard_input', '0')
